#include <ros/ros.h>
#include <koala_kinematics/Empty_Int.h>
#include <koala_kinematics/Int_Bool.h>
#include <std_msgs/Int32.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "Koala_TEST");

    ros::NodeHandle n;
    ros::ServiceClient client = n.serviceClient<koala_kinematics::Int_Bool>("/koala/drive_distance");
    ros::Publisher turn_angle_pub = n.advertise<std_msgs::Int32>("/koala/turn_angle_pub", 100);
    ros::Publisher drive_distance_pub = n.advertise<std_msgs::Int32>("/koala/drive_distance_pub", 100);



    std_msgs::Int32 pub_angle;
    std_msgs::Int32 pub_distance;

    pub_angle.data = 10;
    pub_distance.data = 600;

    while(ros::ok())
    {
        turn_angle_pub.publish(pub_angle);
        //drive_distance_pub.publish(pub_distance);
        sleep(5);
        std::cout << "NOL:JSNJOKNK" <<std::endl;
        ros::spinOnce();
    }
//    koala_kinematics::Int_Bool srv;
//    srv.request.req = -800;

//    ros::ServiceClient client = n.serviceClient<koala_kinematics::Int_Bool>("/koala/turn_angle");
//    koala_kinematics::Int_Bool srv;
//    srv.request.req = -90;

//    if (client.call(srv))
//    {
//        std::cout << "KOALA MOVED: " << srv.response.res << std::endl;
//    }
//    else
//    {
//      ROS_ERROR("Failed to call service koala drive distance");
//      return 1;
//    }

    return 0;

  return 0;
}//main
